﻿using GeoJSON.Net.Feature;
using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Logging;
using Model.Core.MqModels;
using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using System;
using System.Threading.Tasks;

namespace Model.Core.Services
{
    public class JxNavClient:IDisposable
    {
        public WebSocketHelper _WebSocketHelper { get; set; }
        public IHttpClientHelper _HttpClientHelper { get; }
        public IConfiguration _Configuration { get; }
        string ServerIp { get; set; }
        int ServerPort { get; set; }
        string WebSocketUri { get; set; }
        public string MapUid { get; set; }
        public string MapName { get; set; }

        public WheelState CurrentWheelState { get; set; }
        
        public Action<double, double, double> ActionPointInfo { get; set; }

        public Action<string> ActionWheelStateInfo { get; set; }

        public (bool Ischarge, int num) BatteryState;
        public JxNavClient(WebSocketHelper webSocketHelper, IHttpClientHelper httpClientHelper, IConfiguration configuration, ILogger<JxNavClient> logger)
        {
            BatteryState = (false, 0);
            _WebSocketHelper = webSocketHelper;
            _HttpClientHelper = httpClientHelper;
            _Configuration = configuration;
            _logger = logger;

            InitWebSocket();
        }

        public bool IsConnectedHealthy { get; set; }
        public void InitWebSocket()
        {

            IConfiguration configuration = _Configuration.GetSection("MapNavOption");
            ServerIp = configuration["ServerIp"];
            ServerPort = Convert.ToInt32(configuration["ServerPort"]);
            MapUid = configuration["MapUid"];
            MapName = configuration["MapName"];
            string UriPath = configuration["Path"];

            _WebSocketHelper.RecvDataAction = _ClientHelper_RecvDataAction;
            _WebSocketHelper.OnConnectedAction = WebSocketOnConnectFunc;
            _WebSocketHelper.ConnectedStatusAction = a =>
            {
                IsConnectedHealthy = a;
            };
            WebSocketUri = $"ws://{ServerIp}:{ServerPort}{UriPath}";
            _WebSocketHelper.ConnectAsync(WebSocketUri);
            _logger.LogDebug("Initial WebSocket connection status: {ConnectionStatus}", _WebSocketHelper.isConnect());
        }


        double[] CurrentPointInfo { get; set; } = new double[3];

        private void WebSocketOnConnectFunc(bool obj)
        {
            _logger.LogInformation($"WebSocket connection established. Status: {obj}", obj);
            if (obj)
            {
                //增加监听定位状态
                _WebSocketHelper.SendMsg("{\"enable_topic\": \"/slam/state\"}");
                //增加监听当前位姿
                _WebSocketHelper.SendMsg("{\"enable_topic\": \"/tracked_pose\"}");
                _WebSocketHelper.SendMsg("{\"enable_topic\": \"/planning_state\"}");
                _WebSocketHelper.SendMsg("{\"enable_topic\": \"/battery_state\"}");
                _WebSocketHelper.SendMsg("{\"enable_topic\": \"/wheel_state\"}");
                // 重定位消息
                _WebSocketHelper.SendMsg("{\"enable_topic\": \"/global_positioning_state\"}");
            }
        }

        public void ChangeMap()
        {
            string urimap = $"http://{ServerIp}:{ServerPort}/chassis/current-map";
            var curmapdata = _HttpClientHelper.GetAsync(urimap).Result;
            JObject jobject = JsonConvert.DeserializeObject<JObject>(curmapdata);
            Console.WriteLine(jobject.ToString());
            string mapid = jobject["uid"].ToString();
            SetCurrentMapFunc(mapid);
        }
        /// <summary>
        /// 设置当前地图
        /// </summary>
        public async Task SetCurrentMapFunc(string mapname)
        {
            JObject jsoncudata = new JObject();
            jsoncudata.Add("map_uid", mapname);

            string ctrlMode_Url = $"http://{ServerIp}:{ServerPort}/chassis/current-map";
            _ = _HttpClientHelper.PostAsync(ctrlMode_Url, JsonConvert.SerializeObject(jsoncudata));
            _logger.LogInformation("Initiating map configuration change");
            _logger.LogDebug("Map change payload: {Payload}", jsoncudata);

            await Task.Delay(500);

            JObject cmd = new JObject();
            cmd["position"] = new JArray()
                {
                    CurrentPointInfo[0],CurrentPointInfo[1],0
                };

            cmd["ori"] = CurrentPointInfo[2];
            cmd["adjust_position"] = true;
            cmd["message"] = "task server global relocation";

            string UriStr = $"http://{ServerIp}:{ServerPort}/chassis/pose";
            string data = JsonConvert.SerializeObject(cmd);
            _ = _HttpClientHelper.PostAsync(UriStr, data);

        }

        private readonly ILogger _logger;
        private bool _wait_relocation = false;
        public void global_relocation()
        {
            string ctrlMode_Url = $"http://{ServerIp}:{ServerPort}/services/start_global_positioning";
            _wait_relocation = true;
            _ = _HttpClientHelper.PostAsync(ctrlMode_Url, "");
        }


        private void _ClientHelper_RecvDataAction(string obj)
        {
            if (string.IsNullOrEmpty(obj)) return;
            JObject jsdata = JsonConvert.DeserializeObject<JObject>(obj);
            string topicstr = jsdata["topic"]?.ToString();
            if (string.IsNullOrEmpty(topicstr)) return;
            ///获取当前坐标
            GetRobotPoints(topicstr, jsdata);
            ///获取当前移动状态
            GetPlanningState(topicstr, jsdata);
            ///获取当前电池状态
            GetBatteryState(topicstr, jsdata);
            // 获得重定位坐标后重定位
            GetGlobal_positioning_state(topicstr, jsdata);
            // 获得轮控状态
            GetWheelState(topicstr, jsdata);

        }
        /// <summary>
        ///获取轮子状态
        /// </summary>
        /// <param name="topicstr"></param>
        /// <param name="jsdata"></param>
        /// <exception cref="NotImplementedException"></exception>
        private void GetWheelState(string topicstr, JObject jsdata)
        {
            try
            {
                if (!topicstr.Equals("/wheel_state")) return;
                ActionWheelStateInfo?.Invoke(JsonConvert.SerializeObject(jsdata));
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "获取轮子状态异常");
            }            
        }
        public void SetWheelState(string  state)
        {
            var controlMode = new JObject { ["control_mode"] = state };
            string ctrlMode_Url = $"http://{ServerIp}:{ServerPort}/services/wheel_control/set_control_mode";
            _ = _HttpClientHelper.PostAsync(ctrlMode_Url, JsonConvert.SerializeObject(controlMode));
        }
        /// <summary>
        /// 获取当前电池状态
        /// </summary>
        /// <param name="topicstr"></param>
        /// <param name="jsdata"></param>
        private void GetBatteryState(string topicstr, JObject jsdata)
        {
            // throw new NotImplementedException();
            if (!topicstr.Equals("/battery_state")) return;
            string state = jsdata["power_supply_status"].ToString();
            int batteryNum = (int)(Convert.ToDouble(jsdata["percentage"]) * 100);

            if (state.Equals("charging") || state.Equals("full"))
                BatteryState.Ischarge = true;
            else
                BatteryState.Ischarge = false;
        }

        private void GetGlobal_positioning_state(string topicstr, JObject jsdata)
        {
            if (!_wait_relocation) return;
            _wait_relocation = false;
            if (!topicstr.Equals("/global_positioning_state")) return;
            string state = jsdata["state"]?.ToString();
            if ("succeeded".Equals(state))
            {
                var pose = jsdata["pose"];
                var pointarry = pose["pos"]?.ToArray();
                double x = (double)pointarry[0];
                double y = (double)pointarry[1];
                double z = (double)pose["ori"];
                CurrentPointInfo[0] = Convert.ToDouble(pointarry[0]);
                CurrentPointInfo[1] = Convert.ToDouble(pointarry[1]);
                CurrentPointInfo[2] = Convert.ToDouble(pose["ori"]);
                double score = (double)jsdata["score"];
                string needs_confirmation = jsdata["needs_confirmation"]?.ToString();
                _logger.LogDebug("Received global positioning state: X:{X}, Y:{Y}, Z:{Z}, Score:{Score}, ConfirmationNeeded:{Confirmation}", x, y, z, score, needs_confirmation);
                JObject cmd = new JObject();
                cmd["position"] = new JArray()
                {
                    x, y,0
                };

                cmd["ori"] = z;
                cmd["adjust_position"] = true;
                cmd["message"] = "task server map select end";

                string UriStr = $"http://{ServerIp}:{ServerPort}/chassis/pose";
                string data = JsonConvert.SerializeObject(cmd);
                _ = _HttpClientHelper.PostAsync(UriStr, data);

                _logger.LogDebug("Sending global relocation command: {Command}", cmd);
            }
        }

        public Action<bool, string> MoveStatusAction { get; set; }
        public WebSocketHelper WebSocketHelper { get; }

        private void GetPlanningState(string topic, JObject jsdata)
        {
            //  throw new NotImplementedException();
            if (!topic.Equals("/planning_state")) return;

            var pointarrive = jsdata["move_state"].ToString();
            if (pointarrive.Equals("succeeded"))
            {
                string targetid = jsdata["action_id"].ToString();
                MoveStatusAction?.Invoke(true, targetid);
            }

        }

        private void GetRobotPoints(string topic, JObject data)
        {
            if (!topic.Equals("/tracked_pose")) return;

            var pointarry = data["pos"]?.ToArray();
            CurrentPointInfo[0] = Convert.ToDouble(pointarry[0]);
            CurrentPointInfo[1] = Convert.ToDouble(pointarry[1]);
            CurrentPointInfo[2] = Convert.ToDouble(data["ori"]);
            ActionPointInfo?.Invoke((double)pointarry[0], (double)pointarry[1], (double)data["ori"]);
        }




        /// <summary>
        /// 获取输入点数据
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        /// <param name="pointType">0:默认：1:不包含角度 2：只包含角度</param>
        /// <returns></returns>
        public JObject GetInputPointData(double x, double y, double z, int pointType)
        {
            var request = new JObject();
            switch (pointType)
            {
                case 0:
                    request.Add("target_x", x);
                    request.Add("target_y", y);
                    request.Add("target_ori", z);
                    break;
                case 1:
                    request.Add("target_x", x);
                    request.Add("target_y", y);
                    break;
                case 2:
                    request.Add("target_ori", z);
                    break;
            }
            return request;
        }

        public async Task<int>  SendMovePoint(RobotMoveType movetype, JObject pointdata)
        {
            var controlMode = new JObject { ["control_mode"] = "auto" };
            string ctrlMode_Url = $"http://{ServerIp}:{ServerPort}/services/wheel_control/set_control_mode";
            _ = _HttpClientHelper.PostAsync(ctrlMode_Url, JsonConvert.SerializeObject(controlMode));
            JObject jsondata = new JObject();
            string typestr = string.Empty;
            switch (movetype)
            {
                case RobotMoveType.SimpleMove: typestr = "standard"; break;
                case RobotMoveType.UpLoadBoxThenMove: typestr = "align_with_rack"; break;
                case RobotMoveType.UnLoadBoxAfterMove: typestr = "to_unload_point"; break;
                case RobotMoveType.along_given_route: typestr = "along_given_route"; break;
            }
            foreach (var item in pointdata)
            {
                jsondata.Add(item.Key.ToString(), item.Value.ToString());
            }
            jsondata.Add("type", typestr);
            jsondata.Add("creator", "robot-admin-web");
            if (movetype == RobotMoveType.along_given_route)
            {
                double x = Convert.ToDouble(pointdata["target_x"]);
                double y = Convert.ToDouble(pointdata["target_y"]);
                double[] points = new double[] { CurrentPointInfo[0], CurrentPointInfo[1], x, y };
                string pointstr = string.Join(",", points);
                jsondata.Add("route_coordinates", pointstr);
            }
            string UriStr = $"http://{ServerIp}:{ServerPort}/chassis/moves";
           
            string data = JsonConvert.SerializeObject(jsondata);
            string reslut =await _HttpClientHelper.PostAsync(UriStr, data);
            int id = 0;
            if (!string.IsNullOrEmpty(reslut))
            {
                JObject respancejson = JsonConvert.DeserializeObject<JObject>(reslut);
                 id = Convert.ToInt32(respancejson["id"]);
                _logger.LogInformation("Sending move movtype:{typestr} PointData:{pointdata}Sucessed id:{id}", typestr, JsonConvert.SerializeObject(pointdata),id);
            }
            return id;
        }

        public async Task<string> GetMovePlanState(string moveid)
        {
            string UriStr = $"http://{ServerIp}:{ServerPort}/chassis/moves/{moveid}";
            var CURMAPDATA = await _HttpClientHelper.GetAsync(UriStr);            
            return CURMAPDATA;
        }

        public void PatchMapApi(string mapdata)
        {
            #region 获取当前地图信息
            int mapid = GetCurMapUid();
            string UriStr = $"http://{ServerIp}:{ServerPort}/maps/{mapid}";
            var CURMAPDATA = _HttpClientHelper.GetAsync(UriStr).Result;

            _logger.LogDebug("地图信息详情：\n{CURMAPDATA}", CURMAPDATA);
            #endregion
            #region 根据地图更新点位

            var CurallMapDataObj = JsonConvert.DeserializeObject<JObject>(CURMAPDATA);
            if (CurallMapDataObj == null) { _logger.LogError("解析Global地图信息异常"); return; }
            FeatureCollection UpdateObject = JsonConvert.DeserializeObject<FeatureCollection>(mapdata);
            FeatureCollection overlaysFeaturesObj = new();
            var overlays = CurallMapDataObj["overlays"];
            if (overlays != null)
            {

                var overlaysString = overlays.ToString();
                overlaysFeaturesObj = !string.IsNullOrEmpty(overlaysString) ? JsonConvert.DeserializeObject<FeatureCollection>(overlaysString) : new FeatureCollection();
                _logger.LogInformation("Overlays特征数量: {FeatureCount}", overlaysFeaturesObj?.Features.Count);
                try
                {
                    if (UpdateObject?.Features != null)
                    {
                        _logger.LogInformation("更新对象数量: {UpdateCount}", UpdateObject.Features.Count);
                        overlaysFeaturesObj?.Features.RemoveAll(item =>
                        {
                            var properties = item.Properties;
                            if (properties == null) return false;

                            properties.TryGetValue("name", out var nameObj);
                            var hasType = properties.TryGetValue("type", out var typeObj);

                            var geometryType = item.Geometry?.Type.ToString();
                            var typeValue = hasType ? typeObj?.ToString() : null;

                            _logger.LogInformation("特征检查: {{GeometryType: {GeometryType}, Type: {Type}, Name: {Name}}}",
                                geometryType, typeValue, nameObj?.ToString());

                            return ShouldRemoveFeature(geometryType, typeValue);
                        });

                        List<Feature>? list = UpdateObject?.Features;
                        if (list != null)
                        {
                            for (int i = 0; i < list.Count; i++)
                            {
                                Feature? item = list[i];
                                overlaysFeaturesObj?.Features.Add(item);
                            }
                        }
                    }
                    else
                    {
                        _logger.LogWarning("UpdateObject 或其 Features 属性为空，无法获取更新对象数量。");
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "导航客户端发生异常");
                }
            }
            else
            {
                //overlaysFeaturesObj.Features = new List<Feature>();
                foreach (var item in UpdateObject.Features)
                {
                    overlaysFeaturesObj?.Features.Add(item);
                }
            }

            JObject Cuobject = new JObject();
            Cuobject["map_name"] = CurallMapDataObj["map_name"];

            Cuobject["overlays"] = JsonConvert.SerializeObject(overlaysFeaturesObj);
            _logger.LogInformation("{CuobjectData}", Cuobject.ToString());
            _ = _HttpClientHelper.PatchAsync(UriStr, JsonConvert.SerializeObject(Cuobject));
            ChangeMap();
            #endregion
        }

        bool ShouldRemoveFeature(string geometryType, string typeValue)
        {
            return string.Equals(geometryType, "point", StringComparison.OrdinalIgnoreCase)
                && typeValue == "34";
        }
        public int GetCurMapUid()
        {
            #region 获取当前地图id
            string urimap = $"http://{ServerIp}:{ServerPort}/chassis/current-map";
            var curmapdata = _HttpClientHelper.GetAsync(urimap).Result;
            JObject jobject = JsonConvert.DeserializeObject<JObject>(curmapdata);
            int mapid = Convert.ToInt32(jobject["id"]);
            return mapid;
            #endregion
        }

        /// <summary>
        /// 移动速度 /services/wheel_control/set_control_mode
        /// </summary>
        /// <param name="x"></param>
        /// <param name="z"></param>
        public void SendMoveSpeed(double x, double z)
        {
            JObject sendObject = new JObject()
            {
                ["topic"] = "/twist",
                ["linear_velocity"] = x,
                ["angular_velocity"] = z,
            };

            var jsonString = JsonConvert.SerializeObject(sendObject);
            _logger.LogDebug("Sending movement command: {Command}", jsonString);
            _WebSocketHelper.SendMsg(jsonString);
        }

        #region 移动控制
        private async Task<bool> MoveWithTimeout(double speed, double distance, int timeoutSeconds, CancellationToken cancellationToken)
        {
            try
            {
                var ctrlModeUrl = $"http://{ServerIp}:{ServerPort}/services/wheel_control/set_control_mode";
                var modePayload = new JObject { ["control_mode"] = "remote" };

                _ = _HttpClientHelper.PostAsync(ctrlModeUrl, JsonConvert.SerializeObject(modePayload));
                _logger.LogInformation("切换至远程控制模式");

                var timeoutMs = distance > 0
                    ? Math.Abs((int)((distance / speed) * 1000))
                    : timeoutSeconds * 1000;
                _logger.LogDebug("移动参数 速度:{Speed} 距离:{Distance} 超时:{Timeout}ms",
                    speed, distance, timeoutMs);
                var startTime = DateTime.Now;
                while ((DateTime.Now - startTime).TotalMilliseconds <= timeoutMs)
                {
                    cancellationToken.ThrowIfCancellationRequested();
                    SendMoveSpeed(speed, 0);
                    await Task.Delay(50, cancellationToken);
                }
                //_logger.LogWarning("移动操作已");
                return true;
            }
            catch (OperationCanceledException)
            {
                _logger.LogWarning("移动操作已被取消");
                return false;
            }
            finally
            {
                _logger.LogInformation("移动操作结束 最终速度:{Speed}", speed);
            }
        }
        /// <summary>
        /// 后退
        /// </summary>
        /// <param name="speed"></param>
        /// <param name="distance"></param>
        /// <param name="timeoutSeconds"></param>
        /// <param name="ct"></param>
        /// <returns></returns>
        public Task<bool> StartMoveReTreat(double speed, double distance, int timeoutSeconds, CancellationToken ct)
            => MoveWithTimeout(-Math.Abs(speed), distance, timeoutSeconds, ct);
        /// <summary>
        /// 前进
        /// </summary>
        /// <param name="speed"></param>
        /// <param name="distance"></param>
        /// <param name="timeoutSeconds"></param>
        /// <param name="ct"></param>
        /// <returns></returns>
        public Task<bool> StartMoveAdvance(double speed, double distance, int timeoutSeconds, CancellationToken ct)
            => MoveWithTimeout(Math.Abs(speed), distance, timeoutSeconds, ct);
        #endregion


        #region 停止导航
        public void StopNav()
        {
            JObject jObject = new JObject { ["state"] = "cancelled" };
            string UriStr = $"http://{ServerIp}:{ServerPort}/chassis/moves/current";
            string data = JsonConvert.SerializeObject(jObject);
            _ = _HttpClientHelper.PatchAsync(UriStr, data);
        }

        public void Dispose()
        {
            _WebSocketHelper?.DisposeAsync();
        }
        #endregion

    }

    /// <summary>
    /// 机器人移动类型
    /// </summary>
    enum ActionType
    {
        none,
        standard,
        charge,
        along_given_route, // move along a given track
        return_to_elevator_waiting_point, // used when failed to enter elevator
        enter_elevator,
        leave_elevator,
        pull_over, // (DON'T USE) pull over to make space (for other robots to pass)
        align_with_rack, // (DON'T USE)
    }
    /// <summary>
    /// 移动状态
    /// </summary>
    enum MoveState
    {
        none,
        idle,
        moving,
        succeeded,
        failed,
        cancelled,
    }
    public class WheelState
    {
        /// <summary>
        /// 控制模式  //auto/remote/manual
        /// </summary>
        public string? ControlMode { get; set; }
        
        /// <summary>
        /// 急停状态
        /// </summary>
        public bool EmergencyStopPressed { get; set; }

    }
}
